#include "mqfs.h"
#include <rtdevice.h>

#define mqfs_lock(d) rt_mutex_take(&d->lock, -1)
#define mqfs_unlock(d) rt_mutex_release(&d->lock)
#define mqfs_dirent_add(r, d) rt_list_insert_before(&r->dlist, &d->dlist)

#define MQ_MAX_BYTES   100*1024
#define MQ_MAX_MSGS    64
#define MQ_PRIO_MAX    32767

static struct mqfs_dirent* mqfs_dirent_find(struct mqfs_dirent *root, const char *name)
{
    struct mqfs_dirent *d = 0, *pos;

    rt_list_for_each_entry(pos, struct mqfs_dirent, &root->dlist, dlist)
    {
        if (rt_strcmp(pos->name, name) == 0)
        {
            d = pos;
            break;
        }
    }

    return d;
}

struct mqfs_dirent* mqfs_dirent_alloc(const char *name, int mode, struct mq_attr *attr)
{
    struct mqfs_dirent *d;

    if (!attr || (attr->mq_maxmsg > MQ_MAX_MSGS) || (attr->mq_msgsize > MQ_MAX_BYTES))
        return 0;

    d = rt_calloc(1, sizeof(struct mqfs_dirent));
    if (d)
    {
        rt_list_init(&d->dlist);
        rt_list_init(&d->mlist);
        rt_mutex_init(&d->lock, "mq", 0);
        rt_strncpy(d->name, name, sizeof(d->name) - 1);
        d->maxmsgs = attr->mq_maxmsg;
        d->maxlen = attr->mq_msgsize;
        rt_wqueue_init(&d->rwq);
        rt_wqueue_init(&d->wwq);
    }

    return d;
}

int mqfs_open(struct mqfs_dirent *root, const char *name, int oflags, 
                     int mode, struct mq_attr *attr, struct mqfs_dirent **dst)
{
    int ret = 0;
    struct mqfs_dirent *d;

    mqfs_lock(root);
    d = mqfs_dirent_find(root, name);
    if (d)
    {
        if ((oflags & (O_CREAT | O_EXCL)) == (O_CREAT | O_EXCL))
        {
            ret = -EEXIST;
            goto _out;
        }

        d->ref_cnt ++;
        *dst = d;
    }
    else
    {
        if ((oflags & O_CREAT) == 0)
        {
            ret = -ENOENT;
            goto _out;
        }

        d = mqfs_dirent_alloc(name, mode, attr);
        if (!d)
        {
            ret = -ENOSPC;
            goto _out;
        }
        d->ref_cnt = 1;
        mqfs_dirent_add(root, d);
        *dst = d;
    }

_out:
    mqfs_unlock(root);

    return ret;
}

struct mqfs_msg* mqfs_msg_alloc(unsigned size, unsigned prio)
{
    struct mqfs_msg* m;

    if (size > MQ_MAX_BYTES)
        return 0;

    m = rt_malloc(sizeof(*m) + size);
    if (m)
    {
        m->len = size;
        m->prio = prio;
        m->msg = (char*)m + sizeof(*m);
        rt_list_init(&m->node);
    }

    return m;
}

static void mqfs_msg_free(struct mqfs_msg *m)
{
    rt_list_remove(&m->node);
    rt_free(m);
}

int mqfs_read(struct mqfs_dirent *file, void *buf, size_t count, unsigned *prio)
{
    int len = 0;

    mqfs_lock(file);
    if (!rt_list_isempty(&file->mlist))
    {
        struct mqfs_msg *m;

        m = rt_list_first_entry(&file->mlist, struct mqfs_msg, node);
        if (count < m->len)
        {
            len = -EMSGSIZE;
            goto _out;
        }

        len = m->len;
        rt_memcpy(buf, m->msg, len);
        if (prio)
            *prio = m->prio;
        file->curmsgs --;
        mqfs_msg_free(m);
    }

_out:
    mqfs_unlock(file);

    return len;
}

int mqfs_write(struct mqfs_dirent *file, const void *buf, size_t count, unsigned prio)
{
    struct mqfs_msg *m;

    if (file->curmsgs >= file->maxmsgs)
    {
        return 0;
    }
    if (count > file->maxlen)
    {
        return -EMSGSIZE;
    }

    m = mqfs_msg_alloc(count, prio);
    if (!m)
    {
        return -ENOMEM;
    }

    rt_memcpy(m->msg, buf, count);

    mqfs_lock(file);
    rt_list_insert_before(&file->mlist, &m->node);
    file->curmsgs ++;
    mqfs_unlock(file);

    return (int)count;
}
